
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_state.c
  * @author     baiyang
  * @date       2021-8-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <ahrs/ahrs_view.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void publish_vehicle_home();
/*----------------------------------variable----------------------------------*/
static bool is_set_home;

static uitc_actuator_armed actuator_armed;
static uitc_vehicle_home vehicle_home;
static uitc_vehicle_origin vehicle_origin;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void fms_set_auto_armed(bool b)
{
    // if no change, exit immediately
    if( fms.ap.auto_armed == b )
        return;

    fms.ap.auto_armed = b;
    if(b){
        //AP::logger().Write_Event(LogEvent::AUTO_ARMED);
    }
}

/**
  * @brief       
  * @param[in]   b  
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_actuator_armed_notify(bool b)
{
    uint64_t timestamp_us = time_micros64();

    notify_flags.armed = b;

    brd_set_soft_armed(b);

    if (b) {
        publish_vehicle_home();
    }

    actuator_armed.timestamp_us = timestamp_us;
    actuator_armed.armed = b;
    itc_publish(ITC_ID(vehicle_actuator_armed), &actuator_armed);
}

void fms_update_home()
{
    if (is_set_home) {
        return;
    }

    if (itc_copy_from_hub(ITC_ID(vehicle_origin), &vehicle_origin) == 0
        && vehicle_origin.valid_lpos) {
        publish_vehicle_home();
    }
}

void fms_update_using_interlock()
{
    // check if we are using motor interlock control on an aux switch or are in throw mode
    // which uses the interlock to stop motors while the copter is being thrown
    fms.ap.using_interlock = RCs_find_channel_for_option(RC_MOTOR_INTERLOCK) != NULL;
}

// ---------------------------------------------
void fms_set_failsafe_radio(bool b)
{
    // only act on changes
    // -------------------
    if(fms.failsafe.radio != b) {

        // store the value so we don't trip the gate twice
        // -----------------------------------------------
        fms.failsafe.radio = b;

        if (fms.failsafe.radio == false) {
            // We've regained radio contact
            // ----------------------------
            fms_failsafe_radio_off_event();
        }else{
            // We've lost radio contact
            // ------------------------
            fms_failsafe_radio_on_event();
        }

        // update AP_Notify
        notify_flags.failsafe_radio = b;
    }
}


// ---------------------------------------------
void fms_set_failsafe_gcs(bool b)
{
    fms.failsafe.gcs = b;

    // update AP_Notify
    notify_flags.failsafe_gcs = b;
}

/// 
static void publish_vehicle_home()
{
    vehicle_home.timestamp_us = time_micros64();

    vehicle_home.valid_alt = fms.ahrs->valid_alt;
    vehicle_home.valid_hpos = fms.ahrs->valid_hpos;
    vehicle_home.valid_lpos = fms.ahrs->valid_lpos;

    vehicle_home.alt = fms.ahrs->curr_alt_amsl;
    vehicle_home.z   = fms.ahrs->home_alt_above_origin;

    vehicle_home.lat = fms.ahrs->valid_hpos ? fms.ahrs->curr_loc.lat : 0;
    vehicle_home.lon = fms.ahrs->valid_hpos ? fms.ahrs->curr_loc.lng : 0;

    vehicle_home.x = fms.ahrs->relpos_cm.x * 0.01f;
    vehicle_home.y = fms.ahrs->relpos_cm.y * 0.01f;

    vehicle_home.yaw = fms.ahrs->yaw;
    vehicle_home.mag_decl = fms.ahrs->valid_hpos ? geo_get_declination((double)vehicle_home.lat*1e-7, (double)vehicle_home.lon*1e-7) : 0;

    itc_publish(ITC_ID(vehicle_home), &vehicle_home);

    is_set_home = true;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    console_printf("publish_vehicle_home: lat:%d, lon:%d, alt: %f\n", vehicle_home.lat, vehicle_home.lon,
                                                   vehicle_home.alt);
#endif
}
/*------------------------------------test------------------------------------*/


